#ifndef HW_OPENGL_TRAJECTORYDRAWER_H
#define HW_OPENGL_TRAJECTORYDRAWER_H
#include <vector>
#include <iostream>
#include <math.h>
#include <utility>
#include <thread>
#include <mutex>
#include <chrono>
#include <functional>
#include <atomic>
#include <stdio.h>
#include <GL/glut.h>
#include <GL/gl.h>
#include <sstream>

std::vector<float> view_x_,view_y_,view_z_;
namespace draw3d {

    typedef std::vector<float>* PointPtr;

    PointPtr px;
    PointPtr py;

    PointPtr pz;

    GLint SCREEN_WIDTH=0;
    GLint SCREEN_HEIGHT=0;

    GLint windowWidth=800;
    GLint windowHeight=800;

    GLfloat xRotAngle=-75.0f;

    GLfloat yRotAngle=0.0f;

    GLfloat zRotAngle=-135.0f;



    float MIN_X = -200;
    float MAX_X = 200;

    float MIN_Y = -200;
    float MAX_Y = 200;

    float MIN_Z = -200;
    float MAX_Z = 200;


    GLfloat coordinatesize=200.0f;
    GLfloat ratio =1;

    float ScaleX = 800/(MAX_X-MIN_X);
    float ScaleY = 800/(MAX_Y-MIN_Y);

    float ScaleZ = 800/(MAX_Z-MIN_Z);

    void init(void) {
        glClearColor(0.0, 0.0, 0.0, 0.0);        
        glShadeModel(GL_FLAT);                
    }


    void drawLine(float x1, float y1, float z1, float x2, float y2, float z2) {
        glBegin(GL_LINES);
        glVertex3f(x1, y1, z1);
        glVertex3f(x2, y2, z2);
        glEnd();
        glFlush();
    }

    void draw() {

        glColor3f(1, 0, 0);
        drawLine(0, 0, 0, MAX_X, 0, 0); 

        glColor3f(0, 1, 0);
        drawLine(0, 0, 0, 0, MAX_Y, 0); 

        glColor3f(0, 0, 1);
        drawLine(0, 0, 0, 0, 0, MAX_Z); 

        glColor3f(1, 1, 0);

        if (px->size() < 2)
            drawLine(0, 0, 0, 0, 0, 0);
        else
            for (int i = 0; i < px->size() - 1; i++) {
                drawLine((*px)[i], (*py)[i], (*pz)[i],(*px)[i + 1], (*py)[i + 1],(*pz)[i+1]);
            }

        glEnd();
        glPopMatrix();
        glutSwapBuffers();

    }

    void display(void) {

        //std::cout << "start show" << std::endl;
        glClear(GL_COLOR_BUFFER_BIT);            
        glPushMatrix();

        glRotatef(xRotAngle,1.0f,0.0f,0.0f);

        glRotatef(yRotAngle,0.0f,1.0f,0.0f);

        glRotatef(zRotAngle,0.0f,0.0f,1.0f);

        draw();

    }


    void reshapeOperate(){
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        if(ratio<1)
            glOrtho(-coordinatesize,coordinatesize,-coordinatesize/ratio,coordinatesize/ratio,-coordinatesize,coordinatesize);
        else
            glOrtho(-coordinatesize*ratio,coordinatesize*ratio,-coordinatesize,coordinatesize,-coordinatesize,coordinatesize);

        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();

    }

    void reshape(int w, int h) {

        if((w==0)||(h==0))
            return;

        glViewport(0,0,w,h);

        ratio=(GLfloat)w/(GLfloat)h;

        reshapeOperate();

    }


    void myIdle(void){
        display();
    }

    void sPecialkeyFuc(int key,int x,int y){

        if(key==GLUT_KEY_UP){
            xRotAngle-=5.0f;
        }
        else if(key==GLUT_KEY_DOWN){
            xRotAngle+=5.0f;
        }
        else if(key==GLUT_KEY_LEFT){
            yRotAngle-=5.0f;
        }
        else if(key==GLUT_KEY_RIGHT){
            yRotAngle+=5.0f;
        }
        else if(key==GLUT_KEY_PAGE_UP)
            zRotAngle+=5.0f;
        else if(key == GLUT_KEY_PAGE_DOWN)
            zRotAngle-=5.0f;

        glutPostRedisplay();
    }

    void processMouse(int button, int state, int x, int y)
    {

        std::cout << state << "  "  << button  << std::endl;
        if (state == GLUT_UP && button == 3)
        {
            coordinatesize =coordinatesize -10;

        }
        if (state == GLUT_UP && button == 4)
        {
            coordinatesize = coordinatesize +10;

        }
        if(coordinatesize <0)
            coordinatesize=0;
        reshapeOperate();

    }


    void initial(std::vector<float> &x, std::vector<float> &y, std::vector<float> &z) {
        int argc;
        char **argv;
        px = &x;
        py = &y;
        pz = &z;
        glutInit(&argc, argv);
        glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
        glutCreateWindow("draw3D");
        glutReshapeWindow(windowWidth,windowHeight);
        glutPositionWindow((SCREEN_WIDTH-windowWidth)/2,(SCREEN_HEIGHT-windowHeight)/2);
        init();
        glutDisplayFunc(display);
        glutReshapeFunc(reshape);
        glutSpecialFunc(sPecialkeyFuc);
        glutIdleFunc(&myIdle);
        glutMouseFunc(processMouse);
        glutMainLoop();
    }

};

#endif //HW_OPENGL_TRAJECTORYDRAWER_H
